/*
 **********************************************************************
 * File name: dcm.h
 *
 * COPYRIGHT (C) 2018-2022
 *
 * The license and distribution terms for this file may be found in 
 * the LICENSE file.
 *
 * Author: ChenLong (Add new editor)
 * Email: worldlong@foxmail.com
 **********************************************************************
*/
#pragma once
/*
*/
#include <cmath>
#include <cstring>
#include <cstdint>
#include "matrix.h"
#include "vector.h"
#include "euler.h"
#include "quaternion.h"
/*
*/
namespace matrix
{
/*
*/
template<typename Type>
class Euler_T;

template<typename Type>
class Quaternion_T;

template<typename Type>
class Vector3;

/*---------------------------------------
                   DCM_T
       Rotate from global to local
 *----------------------------------------*/
template<typename Type>
class DCM_T : public SquareMatrix<Type, 3>
{
public:
  /*
  */
  DCM_T() {}
  
  DCM_T(const float data0[3][3]) :
    SquareMatrix<Type, 3>(data0)
  {}
  
  DCM_T(const Matrix<Type, 3, 3> &rb) :
    SquareMatrix<Type, 3>(rb)
  {}
  
  DCM_T(const SquareMatrix<Type, 3> &rb) :
    SquareMatrix<Type, 3>(rb)
  {}
  
  DCM_T(const Quaternion_T<Type> &q)
  {
    float q00 = q.q0*q.q0;
    float q11 = q.q1*q.q1;
    float q22 = q.q2*q.q2;
    float q33 = q.q3*q.q3;
    float q01 = q.q0*q.q1;
    float q02 = q.q0*q.q2;
    float q03 = q.q0*q.q3;
    float q12 = q.q1*q.q2;
    float q13 = q.q1*q.q3;
    float q23 = q.q2*q.q3;
    
    this->data[0][0] = q00 + q11 - q22 - q33;
    this->data[0][1] = 2*(q12 + q03);
    this->data[0][2] = 2*(q13 - q02);
    
    this->data[1][0] = 2*(q12 - q03);
    this->data[1][1] = q00 - q11 + q22 - q33;
    this->data[1][2] = 2*(q23 + q01);
    
    this->data[2][0] = 2*(q13 + q02);
    this->data[2][1] = 2*(q23 - q01);
    this->data[2][2] = q00 - q11 - q22 + q33;
  }
  
  DCM_T(const Euler_T<Type> &euler, uint32_t type)
  {
    float sr = sinf(euler.roll);
    float cr = cosf(euler.roll);
    float sp = sinf(euler.pitch);
    float cp = cosf(euler.pitch);
    float sy = sinf(euler.yaw);
    float cy = cosf(euler.yaw);
      
    if(type == 321)  //means rotating order is Z->Y->X
    {
      this->data[0][0] = cp*cy;
      this->data[0][1] = cp*sy;
      this->data[0][2] = -sp;
      
      this->data[1][0] = sr*sp*cy - cr*sy;
      this->data[1][1] = sr*sp*sy + cr*cy;
      this->data[1][2] = sr*cp;
     
      this->data[2][0] = cr*sp*cy + sr*sy;
      this->data[2][1] = cr*sp*sy - sr*cy;
      this->data[2][2] = cr*cp;
    }
    else if(type == 312) //means rotating order is Z->X->Y
    {
      this->data[0][0] = cp*cy - sp*sr*sy;
      this->data[0][1] = cp*sy + sp*sr*cy;
      this->data[0][2] = -sp*cr;
      
      this->data[1][0] = -cr*sy;
      this->data[1][1] = cr*cy;
      this->data[1][2] = sr;
     
      this->data[2][0] = sp*cy + cp*sr*sy;
      this->data[2][1] = sp*sy - cp*sr*cy;
      this->data[2][2] = cp*cr;
    }
  }
  
  /*------------------------------------------*
   * Operator overload                        *
   *------------------------------------------*/
  /*
   * M1 = M2
   */
  DCM_T<Type> &operator=(const DCM_T<Type> &mb)
  {
    if (this != &mb) {
      memcpy(this->data, mb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*
   * M1 = M2
   */
  DCM_T<Type> &operator=(const SquareMatrix<Type, 3> &mb)
  {
    if (this != &mb) {
      memcpy(this->data, mb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*
   * M1 = M2
   */
  DCM_T<Type> &operator=(const Matrix<Type, 3, 3> &mb)
  {
    if (this != &mb) {
      memcpy(this->data, mb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*------------------------------------------*
   * Functions                                *
   *------------------------------------------*/
  inline Vector3<Type> rotate(const Vector3<Type> &vb) const
  {
    return *this * vb;
  }
  
  inline Vector3<Type> rotate_to_local(const Vector3<Type> &vb) const
  {
    return rotate(vb);
  }
  
  inline Vector3<Type> rotate_to_global(const Vector3<Type> &vb) const
  {
    DCM_T<Type> dcm = this->transpose();
    return dcm.rotate(vb);
  }
  
};

typedef DCM_T<float> DCM;    //default DCM is float type

}
